package org.robotfish.geometry;

import org.ejml.simple.SimpleMatrix;


public class TransformPoints {
	
	/**
	 * @param x     3-vector absolute coordinates of (x,y,angle)
	 * @param angle amount to rotate the coordinates by
	 * @return x transformed to new coordinate system {@code angle}. 
	 * Angle of x is copied to a new vector.
	 * Equivalent to rotating clockwise by {@code angle}.  
	 */
	public static SimpleMatrix toRelative( SimpleMatrix x, double angle ) {
		double cosa = Math.cos(angle);
		double sina = Math.sin(angle);
		return new SimpleMatrix( new double[][]{ 
			{ cosa*x.get(0) + sina*x.get(1)}, 
			{-sina*x.get(0) + cosa*x.get(1)}, 
			{ x.get(2) } 
		} );
	}
	
}
